//
// Created by heyijia on 18-12-11.
//

#include "selectScanPoints.h"
#include <iomanip>

int    img_w = 720;
double focal = 450;
double z = 10;  // 在 10m 高处装一个相机，咔咔给激光点云拍照

struct LineSeg
{
    int    id_start;
    int    id_end;
    double dist;
};

std::vector<Eigen::Vector3d> AutoGetLinePts(const std::vector<Eigen::Vector3d> points, bool debug)
{
    // cv::Mat img(img_w, img_w, CV_8UC1, cv::Scalar::all(0));
    cv::Mat img(img_w, img_w, CV_8UC3, cv::Scalar(0, 0, 0));

    // detect and get line
    // 直接从每一帧激光的正前方开始搜索一定距离范围内的符合平面标定板形状的激光线段
    int n = points.size();
    int id = 0;
    // std::cout << points.at(id).transpose() <<" "<<points.at(id+1).transpose() <<std::endl;

    /* 假设每个激光点之间的夹角为0.3deg:
     * step 1:
     * 如果有激光标定板，那么激光标定板必须出现在视野的正前方 120 deg 范围内(通常相机视野也只有 120 deg)，也就是左右各
     * 60deg.
     */

    int delta = 80 / 0.5;

    int id_left = std::min(n - 1 - delta, n - 1);
    int id_right = std::max(id + delta, 0);

    int dist_left = points.at(id_left).norm();
    int dist_right = points.at(id_right).norm();

    // 逻辑别搞复杂了。
    std::vector<LineSeg> segs;
    double               dist_thre = 0.10;
    int                  skip = 3;
    int                  currentPt = id_left;
    int                  nextPt = currentPt + skip;
    int                  continus_pts = 0;
    bool                 newSeg = true;
    LineSeg              seg;

    for (int i = id_left; i < id_right - skip + n - 1; i += skip) {
        if (newSeg) {
            seg.id_start = currentPt;
            seg.id_end = nextPt;
            newSeg = false;
        }

        double d1 = points.at(currentPt).head(2).norm();
        double d2 = points.at(nextPt).head(2).norm();
        double range_max = 100;
        if (d1 < range_max && d2 < range_max) {  // 有效数据,  激光小于 100 m
            if (fabs(d1 - d2) < dist_thre) {     //  8cm
                Eigen::Vector3d dist = points.at(currentPt) - points.at(nextPt);
                if (dist.head(2).norm() > 0.05) {
                    newSeg = true;
                } else {
                    seg.id_end = nextPt;
                }
            } else {
                newSeg = true;
            }

            if (newSeg) {
                seg.id_end -= 1;
                Eigen::Vector3d dist = points.at(seg.id_start) - points.at(seg.id_end);
                if (dist.head(2).norm() > 0.2 && points.at(seg.id_start).head(2).norm() < 2 &&
                    points.at(seg.id_end).head(2).norm() < 2) {  // 至少长于 20 cm, 标定板不能距离激光超过2m
                    continus_pts = (seg.id_end - seg.id_start >= 0) ? (seg.id_end - seg.id_start)
                                                                    : (seg.id_end - seg.id_start + n);
                    if (continus_pts > 30) {  // 标定板上的激光点肯定多余 50 个
                        seg.dist = dist.head(2).norm();
                        segs.push_back(seg);
                        // printf("############## seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end,
                        // dist.head(2).norm());
                    } else {
                        // printf("############## seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end,
                        // dist.head(2).norm());
                    }
                }
            }

            currentPt = nextPt;  // 下一个点
            nextPt = nextPt + skip;
            if (nextPt >= (n - 1)) {
                nextPt = nextPt - (n - 1);
            }
            // if (fabs(currentPt - 710) < 3) {
            //     printf("############## seg start/end: %03d/%03d, d1/d2:%06f/%06f \n", currentPt, nextPt, d1, d2);
            // }
        } else {
            if (d1 > range_max) {
                currentPt = nextPt;  // 下一个点
            }
            nextPt = nextPt + skip;
            if (nextPt >= (n - 1)) {
                nextPt = nextPt - (n - 1);
            }
        }
    }

#if 0
    if (segs.size() == 0) {
        printf("############## seg id_left/id_right: %03d/%03d       111111111111\n", id_left, id_right);
        currentPt = id_left;
        nextPt = currentPt + skip;
        for (int i = id_left; i < id_right - skip + n - 1; i += skip) {

            if(newSeg) {
                seg.id_start = currentPt;
                seg.id_end = nextPt;
                newSeg = false;
            }

            double d1 = points.at(currentPt).head(2).norm();
            double d2 = points.at(nextPt).head(2).norm();
            double range_max = 100;
            continus_pts = 0;
            if(d1 < range_max && d2 < range_max) {  // 有效数据,  激光小于 100 m
                if(fabs(d1-d2) < dist_thre) {         //  8cm
                    seg.id_end = nextPt;
                } else {
                    // printf("############## seg i:%d currentPt/nextPt: %03d/%03d d1/d2:%06f/%06f   33333333\n", i, currentPt, nextPt, d1, d2);
                    newSeg = true;
                    Eigen::Vector3d dist = points.at(seg.id_start) - points.at(seg.id_end);
                    if(dist.head(2).norm() > 0.2
                    && points.at(seg.id_start).head(2).norm() < 2
                    && points.at(seg.id_end).head(2).norm() < 2) {   // 至少长于 20 cm, 标定板不能距离激光超过2m
                        continus_pts = (seg.id_end-seg.id_start >= 0) ? (seg.id_end-seg.id_start) : (seg.id_end-seg.id_start + n);
                        if (continus_pts > 50) { // 标定板上的激光点肯定多余 50 个
                            seg.dist = dist.head(2).norm();
                            segs.push_back(seg);
                            // printf("############## seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end, dist.head(2).norm());
                        } else {
                            // printf("############## seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end, dist.head(2).norm());
                        }
                    }
                    continus_pts = (seg.id_end-seg.id_start >= 0) ? (seg.id_end-seg.id_start) : (seg.id_end-seg.id_start + n);
                    if (continus_pts > 10) {
                        // printf("############## seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end, dist.head(2).norm());
                    }
                }

                currentPt = nextPt;        // 下一个点
                nextPt = nextPt + skip;
                if (nextPt >= (n -1)) {
                    nextPt = nextPt - (n -1);
                }
                if (720 - currentPt < 30 || currentPt < 30) {
                    printf("############## seg id_start:%03d, currentPt/nextPt: %03d/%03d, d1/d2:%06f/%06f continus_pts:%03d\n", seg.id_start, currentPt, nextPt, d1, d2, continus_pts);
                }
            } else {
                //  printf("############## seg i:%d currentPt/nextPt: %03d/%03d 222222\n", i, currentPt, nextPt);
                if(d1 > range_max) {
                    currentPt = nextPt;        // 下一个点
                }
                nextPt = nextPt + skip;
                if (nextPt >= (n -1)) {
                    nextPt = nextPt - (n -1);
                }
            }
        }
    }
#endif

#if 0
    // 对 segs 的边界进行扩充
    for (int i = 0; i < segs.size(); ++i) {
        LineSeg tmp = segs.at(i);
        // printf("############## tmp start/end: %03d/%03d, distance:%06f \n", tmp.id_start, tmp.id_end, tmp.dist);
        for (int j = 1; j < 4; ++j) {
            int boundaryPt = tmp.id_end + j;
            boundaryPt = (boundaryPt < n) ? boundaryPt : (boundaryPt - n);
            double d1 = points.at(tmp.id_end).head(2).norm();
            double d2 = points.at(boundaryPt).head(2).norm();
            if (fabs(d1 - d2) < dist_thre) {  //  8cm
                segs.at(i).id_end = boundaryPt;
            }
        }

        for (int j = -1; j > -4; --j) {
            int boundaryPt = tmp.id_start + j;
            boundaryPt = (boundaryPt >= 0) ? boundaryPt : (boundaryPt + n);
            double d1 = points.at(tmp.id_start).head(2).norm();
            double d2 = points.at(boundaryPt).head(2).norm();
            if (fabs(d1 - d2) < dist_thre) {  //  8cm
                segs.at(i).id_start = boundaryPt;
            }
        }
    }
#endif

    std::vector<Eigen::Vector3d> ptsLine;
    Eigen::Vector3d              lastPoint;
    Eigen::Vector3d              curPoint;
    if (segs.size() > 0) {
        LineSeg bestLine;
        int     maxpts = -1;
        int     idx = 0;
        for (int i = 0; i < segs.size(); ++i) {
            LineSeg tmp = segs.at(i);
            int     cnt = tmp.id_end - tmp.id_start;
            cnt = (cnt >= 0) ? (cnt) : (cnt + n);
            if (cnt > maxpts) {
                bestLine = tmp;
                maxpts = cnt;
            }
        }
        // printf("##############################################\n");
        for (int i = bestLine.id_start; i < bestLine.id_start + maxpts; ++i) {
            idx = i % n;
            curPoint = points.at(idx);
            // if (curPoint.head(2).norm() > 100) {
            //     int half_cnt = 0.5 * (bestLine.id_start + maxpts);
            //     if (i < half_cnt) {
            //         continue;
            //     } else {
            //         break;
            //     }
            // }
            ptsLine.push_back(curPoint);
            // printf("++++++++++++ points[%d] x/y:%f/%f\n", idx, curPoint.x(), curPoint.y());
        }
        // printf("##############################################\n");
    }
    // printf("##############################################\n");
    // lastPoint = points[0];
    // for (int idx = 0; idx < points.size(); idx++) {
    //     auto point = points[idx];
    //     double dist = point.head(2).norm();
    //     if (dist > 100) {
    //         printf("++++++++++++ points[%d] x/y:%f/%f\n", idx, point.x(), point.y());
    //         point.x() = lastPoint.x();
    //         point.y() = lastPoint.y();
    //         printf("------------ points[%d] x/y:%f/%f\n", idx, point.x(), point.y());
    //     }
    //     lastPoint = point;
    // }
    // printf("##############################################\n");

    if (segs.size() != 0 && ptsLine.size() == 0) {
        printf("##############################################\n");
        for (auto seg : segs) {
            printf("############## ptsLine size:0 seg start/end: %03d/%03d, distance:%06f \n", seg.id_start, seg.id_end,
                   seg.dist);
        }
        printf("##############################################\n");
    }

    if (debug) {
        for (auto pt : points) {
            int col = (int)(pt.x() / z * focal + img_w / 2);
            int row = (int)(-pt.y() / z * focal + img_w / 2);  // -Y/Z 加了一个负号, 是为了抵消针孔投影时的倒影效果

            if (col > img_w - 1 || col < 0 || row > img_w - 1 || row < 0) continue;

            cv::Vec3b color_value(255, 0, 0);  // B-G-R
            img.at<cv::Vec3b>(row, col) = color_value;
            // img.at<uchar>(row, col) = 255;
        }

        // for (int j = 0; j < segs.size(); ++j) {
        //     LineSeg tmp = segs.at(j);
        //     int cnt = tmp.id_end - tmp.id_start;
        //     int idx = 0;
        //     cnt = (cnt >= 0) ? (cnt) : (cnt + n);
        //     for (int i = tmp.id_start; i < tmp.id_start + cnt; ++i)
        //     {
        //         idx = i % n;
        //         Eigen::Vector3d pt = points.at(idx);
        //         int col = (int)(pt.x() / z * focal + img_w/2);
        //         int row = (int)(- pt.y() / z * focal + img_w/2);  // -Y/Z 加了一个负号,
        //         是为了抵消针孔投影时的倒影效果

        for (int j = 0; j < ptsLine.size(); ++j) {
            Eigen::Vector3d pt = ptsLine.at(j);
            int             col = (int)(pt.x() / z * focal + img_w / 2);
            int row = (int)(-pt.y() / z * focal + img_w / 2);  // -Y/Z 加了一个负号, 是为了抵消针孔投影时的倒影效果

            if (col > img_w - 1 || col < 0 || row > img_w - 1 || row < 0) continue;

            cv::Vec3b color_value(0, 0, 200);
            img.at<cv::Vec3b>(row, col) = color_value;
        }

        cv::putText(img, "Detecting the Laser Points on the calibra planar!", cvPoint(5, 30),
                    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.7, cvScalar(255, 255, 255), 1, CV_AA);
        cv::imshow("ScanPoint_PRE", img);
        cv::waitKey(10);
    }

    return ptsLine;
}

std::vector<cv::Rect> PointsToImg(const std::vector<Eigen::Vector3d> points, bool selectPoint)
{
    cv::Mat img(img_w, img_w, CV_8UC1, cv::Scalar::all(0));

    for (auto pt : points) {
        int col = (int)(pt.x() / z * focal + img_w / 2);
        int row = (int)(-pt.y() / z * focal + img_w / 2);  // -Y/Z 加了一个负号, 是为了抵消针孔投影时的倒影效果

        if (col > img_w - 1 || col < 0 || row > img_w - 1 || row < 0) continue;

        img.at<uchar>(row, col) = 255;
    }

    std::vector<cv::Rect> rects;
    if (selectPoint) {
        GetRect getrect;
        getrect.gettingROI(img);  // 选取感兴趣的图像区域
        rects = getrect.rects;
    } else {
        cv::imshow("ScanPoint", img);
        cv::waitKey(0);
    }

    return rects;
}

std::vector<Eigen::Vector3d> GetROIScanPoints(const std::vector<Eigen::Vector3d> points,
                                              const std::vector<cv::Rect>        rects)
{
    std::vector<Eigen::Vector3d> RoiPoints;
    for (auto rect : rects) {
        for (auto pt : points) {
            // 将点转换成图像坐标，然后看是否落在感兴趣区域内部
            int col = (int)(pt.x() / z * focal + img_w / 2);
            int row = (int)(-pt.y() / z * focal + img_w / 2);  // -Y/Z 加了一个负号, 是为了抵消针孔投影时的倒影效果
            if (col > rect.x + rect.width || col < rect.x || row > rect.y + rect.height || row < rect.y) continue;
            RoiPoints.push_back(pt);
        }
    }
    return RoiPoints;
}

void GetRect::onMouse(int event, int x, int y, int flags, void *userdata)
{
    // Check for null pointer in userdata and handle the error
    GetRect *temp = reinterpret_cast<GetRect *>(userdata);
    temp->CallBackFunc(event, x, y);
}
void GetRect::CallBackFunc(int event, int x, int y)
{
    if (event == cv::EVENT_LBUTTONDOWN) {
        std::cout << "Left button of the mouse is clicked - position (" << x << ", " << y << ")" << std::endl;

        // Init your rect
        base.x = x;
        base.y = y;
        r.x = x;
        r.y = y;
        r.width = 0;
        r.height = 0;
        bDraw = true;
    } else if (event == cv::EVENT_MOUSEMOVE) {
        //        std::cout << "Mouse move over the window - position (" << x << ", " << y << ")" << std::endl;

        // If drawing, update rect width and height
        if (!bDraw) return;

        int dx = abs(r.x - x);
        int dy = abs(r.y - y);

        if (x < base.x) {
            r.x = x;
            r.width = abs(x - base.x);
        } else {
            r.width = dx;
        }

        if (y < base.y) {
            r.y = y;
            r.height = abs(y - base.y);
        } else {
            r.height = dy;
        }

        // Refresh
        working = layer.clone();
        cv::rectangle(working, r, cv::Scalar(125));
        cv::imshow("ScanPoint", working);
    } else if (event == cv::EVENT_LBUTTONUP) {
        std::cout << "Left button released" << std::endl;

        // Save rect, draw it on layer
        rects.push_back(r);
        cv::rectangle(layer, r, cv::Scalar(125));

        r = cv::Rect();
        bDraw = false;

        // Refresh
        working = layer.clone();
        cv::rectangle(working, r, cv::Scalar(125));
        cv::imshow("ScanPoint", working);
    }
}
void GetRect::gettingROI(cv::Mat img)
{
    layer = img.clone();
    cv::putText(layer, "Please Select ROI scan data with your mouse. Then, Put any key to continue!", cvPoint(5, 30),
                cv::FONT_HERSHEY_COMPLEX_SMALL, 0.7, cvScalar(255), 1, CV_AA);
    cv::namedWindow("ScanPoint", 1);
    cv::imshow("ScanPoint", layer);
    cv::setMouseCallback("ScanPoint", GetRect::onMouse, this);
    cv::waitKey(0);
}
